COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	 RUNTIME SYSTEM
C00006 00003	 TRAJECTORIES
C00010 00004	 JOINT SERVO
C00017 00005	    SCHEDULER
C00024 ENDMK
C⊗;
 RUNTIME SYSTEM
	The runtime system consists of three  elements: 1). A control
computer  together with  a task program.   2).   One or  more general
purpose manipulators.    3). Several  general purpose  sensory  input
devices. We  use tools,jigs  and special markings  to either  make it
possible  to perform a task,  for  example a screw driver. To improve
the efficiency  of  the performance,    for example  use of  a  power
screwdriver.    To  overcome  some  of  our  sensory  and  mechanical
limitations, the lack of  any device equivalent to  a human hand  for
instance, makes  it  advantageous for  us to  use  a screw  dispenser
rather than to attempt to pick screws out of a "barrel."

	Control of  the manipulators is entirely within the computer.
Position,  velocity and force information are read into  the computer
and motor drive level is output by the computer at servo rate.

	The control of such a manipulator could be analog but even in
its  simplest  mode  of  operation,   position  servoing,    it  is a
coordinated motion of all six joints along a trajectory  defined by a
sequence  of polynomials.   In performing  tasks, such a  screwing in
screws,  its motion is a combination of position, velocity and  force
servoing and  it changes  from on mode  to the  other in response  to
external stimulii  interpreted in terms of a plan  of the task.  Even
if many modes of servoing  were provided it would still be  necessary
to provide the  computer with all the servo information  in order for
it to interact  correctly.  Instead  we close all servo  loops within
the computer itself and use no analog servos at all.

	Closing the servo loop  in the computer requires  very little
additional  computation as  the computer  must essentially  perform a
servo calculation in order to decide  under which servo mode to  move
and what set point to output.

	The limitations  imposed by  the computer  are bandwidth  and
digitizing  noise. The  computer servo  is a  sampled data  system of
quite low sample rate.  These limitations can be corrected,  in part,
by  providing  high  frequency  velocity  feedback  in  analog  form,
without affecting the computer servo.
 TRAJECTORIES
	A trajectory for the hand is generated at compile time. It is
assumed that the  hand will pass through the planning time positions.
These positions: departure point, via points, arrival point and final
position are  all assumed to  have their  planning values.   Based on
these values  and the planning time values of all other objects it is
possible to generate a trajectory from the initial point to the final
point  without colliding  with any  other objects.    This trajectory
might be optimized to minimize such things as time or energy.

	If at run time it is found that some or all of these  planned
positions have  moved  slightly it  becomes necessary  to modify  the
planned  trajectory to  pass through  the actual  positions.   If the
actual  positions  are  only  slightly  different  from  the  planned
positions  then  the  trajectory  is still  almost  optimal.  If  the
deviation  from the planned values  to the actual  are great then the
trajectory is  no  longer optimal.  To plan  a  new trajectory  would
optimize the move, but how  much time would it take to compute?  Only
if the  time to  compute is  less than  the time  saved is  it  worth
recomputing. At present this is not the case.

	Before the move is  executed all points that the  hand passes
through are checked to see if they are still valid, that is that they
haven't been changed.  If they are invalid then the  calculator cells
are used to compute a new value.   When the value is obtained the arm
solution program is called to obtain the joint angles. This is stored
with the frame.   Also at  this time the  joint inertias and  gravity
loadings are calculated and stored.  Of course if the frame was valid
then the stored  values would  be used instead.   The  value for  the
pre-computed trajectory are  compared to the newly  obtained solution
and  the differences noted. When  the arm is  moved these differences
will be applied  simultaneously with the trajectory  to obtain a  new
modified trajectory which passes through the actual points.

 JOINT SERVO

	Any coordinated  motion of the  arm can  be expressed as  six
time dependent motions, one for each joint; the coordinated motion is
parameterized in terms of time.

	The problem of servoing the arm, or arms, can be reduced to a
problem of servoing a number of joints each with respect to time.

	A joint  servo has two  parts: a  drive part and  a predictor
part.   At the termination  of the drive part of  the joint servo the
predictor calls the scheduler to reserve time to run this joint servo
again in the future such  as to maintain a minimum sampling rate. The
call to  the scheduler  specifies a  maximum and  a minimum  time  in
milliseconds in the future  between which the joint must  be servoed.
The  scheduler  finds  a  free  time  slot  within these  bounds  and
schedules this joint to be run at that time. It returns the time slot
to the predictor. The predictor now knows at what time the servo will
be run again in the future.

	For  the predicted run  time the polynomial  defining the set
point is evaluated together with any  interpolating polynomial. These
values  added  to  any  offset  give  the predicted  set  point.  The
predicted  velocity  and  acceleration  are  obtained  by  difference
techniques based on recent  set point values.  The  joint inertia and
gravity force loading are interpolated.  The gravity loading is added
to the  predicted acceleration  times the  joint inertia  to yield  a
predicted drive.

	If this joint has multiple  wipers then the appropriate wiper
to  read the joint  position is determined, the  joint calibration is
them applied to the set point to yield a potentiometer equivalent set
point.

	The  servo gains are  determined,   as they are  dependent on
joint inertia, and finally the servo equation is determined in  terms
of observed position and velocity.

	When the  predicted run time  occurs, monitored by  the clock
routine  which dispatches immediately  to the  appropriate servo, the
drive part of the  servo runs. The  drive part measures the  position
and velocity,  evaluates the  servo equation,   applies  the friction
compensation  and drives the  joint. This is a  very fast computation
and minimizes any delay between observation and action. The predictor
is then run again for the next servo scheduling.

	The form  of the servo, position, velocity  or force, affects
the terms of the servo equation. For example if the joint is  running
under a velocity servo then there is no position dependent term.

	The advantage of this servo scheme is that it allows flexible
scheduling, each  joint can run at its  own required repetition rate.
As the  joint knows  when  it will  be  run next  it is  possible  to
pre-compute most of the drive and thus reduce the servo delay.

	Each servo routine has a control block consisting of a status
register,   instantiating interpreter's name and control data. In the
case of  a joint  servo the  status register  contains the  following
bits:

	RUN←←1 ;joint is running or about to be run

	FIRST←←2 ;first time through loop for this motion.
	FINAL←←4 ;in final state, nulling errors
	STOP←←100000 ;stop this joint, or joint is stopped

	EXFORCE←←2000 ;joint stopped due to excessive force.
	ADERR←←4000 ;a/d error
	NONEX←←10000 ;joint is down or does not exist
	STERR←←20000 ;servo was not run on schedule

	SERVO←←10 ;position servo
	VELS←←20 ;velocity servo
	FORCE←←40 ;exert force
	PDIR1←←100 ;with this prefered direction of motion
	PDIR2←←200 ;01 up, 00 stop, 10 down
	WOB←←400 ;perturb this joint while running

	NUL←←1000 ;null errors at end and stop

	The interpreter is called  if the joint stops for any reason,
including the successful  completion of the  motion. The  interpreter
can  determine  the  reason for  stopping  by  examining  the  status
register of the joint.

	The control  data for a joint servo  consists of a pointer to
the set point polynomial  block,  its  set point,  predicted  gravity
torque, actual position velocity, and output torque or force.
    SCHEDULER
Priority levels in the HAL system

	7. AD, joint servoing
	6. Clock, calendar
	5. <spare>
	4. ON condition checkers; Interrupt handlers for
	 various condition-checking devices (eg finger pads).
	3. servo predictor
	2. <spare>
	1. Interpreters, scheduler for background stuff.
	0. Interpreters

Scheduling of servos and other time-dependent events:

1. The HAL  system keeps a  calendar of things  that must be  done in
each time interval.  With time slot is associated:

	a. An AD command list which is to be started.

	b. A queue of procedure calls to make at various priority
	levels.

2. When the clock  ticks, the clock interrupt routine  starts running
in kernel mode.  The first thing it does is to start the specified AD
command list,   if any.   (This  will cause an  AD_done interrupt  in
around 20-30 microseconds.) Finally,  it uses the software interrupts
to arrange for all of the queued procedure calls to take place at the
appropriate levels.

3. The AD service routine runs at priority level 7, and, hence,  uses
register  set zero.    However,   it does  run  in user  mode,   thus
safeguarding the rest of the system  from bugs.  The AD command  list
consists of a sequential table of two word entries:

	a. CHNCMD -- channel command
	b. SAMPLE -- usually, a slot for storing a sample value.

Essentially, the AD  service routine keeps an internal  variable that
points  at the entry  in the command  list corresponding  to the most
recent (ie current)  AD channel command.   When an AD_done  interrupt
occurs,  the service  routine copies  the  contents of  the AD  value
register  into  the current  SAMPLE  slot.   It  then  increments its
pointer to point at  the next two word entry.   If CHNCMD of  the new
entry is non-zero,   it issues the appropriate command and dismisses.
If CHNCMD is zero, then SAMPLE is  assumed to be the address of  some
code to be  executed (typically,   a servo).  In  this case,   the AD
service routine will just clear its internal pointer (to signify that
it is done), and jump to the indicated address.

4. Typically,  a servoing operation will have two "phases", one which
runs  at level  7  as a  response  to an  AD interrupt  and  which is
responsible for emitting the new drive, and a somewhat lower priority
one  which requests  a new  time  slot from  the calendar  management
routine  and  sets up  the correction  phase for  the  new slot.   As
previously indicated, this  first phase is  "scheduled" by putting  a
pointer to the appropriate AD command list into the "AD request" part
of a calendar  time slot.   The second phase  is scheduled merely  by
entering it onto the calendar queue of things  to be requested in the
same tick.

5.    If  a non-critical  process  (eg,   an  "ON"  monitor)  needs a
"consistent" set of AD  measurements, it can  get them by setting  up
the appropriate AD  command list, finding a time slot  for taking the
measurements, and then placing a request that the computation that is
to use the set of  measurements be started up at the time  slot after
the one in which the measurements are made.  

6. In  cases where a  command list is to  long to be  finished in one
time slot, the process requesting  the measurements must reserve  two
(or more) contiguous slots.  This is done by placing the command list
id in  the first slot and a special flag  value (perhaps -1) into the
remaining slots, so that no other processes will try to reserve them.

;Thus, a typical AD request block (for a servo) might be

	chnl1			;FIRST CHANNEL TO BE KICKED
	<val word 1>		;A SLOT FOR VALUE
	chnl2			;SECOND CHANNEL TO BE KICKED
	<val word 2>		;A SLOT FOR VALUE
	0			;FLAG
	JSR R5,SERVO		;THIS GETS JUMPED TO
	<param 1>
	<param 2>
	<param 3>
	:
	<param n>
		  stack is popped.